// Copyright 2024 Polar Bear Team
// Protocol testing tool for drone serial interface

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include "drone_serial_interface/protocol_handler.hpp"

using namespace drone_serial_interface;

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  
  auto logger = rclcpp::get_logger("protocol_test");
  
  RCLCPP_INFO(logger, "Testing Protocol Handlers");
  
  // Create test velocity command
  geometry_msgs::msg::Twist test_cmd;
  test_cmd.linear.x = 1.0;
  test_cmd.linear.y = 0.5;
  test_cmd.linear.z = 0.0;
  test_cmd.angular.z = 0.2;
  
  // Test Custom Protocol
  auto custom_handler = ProtocolFactory::create("custom", logger);
  if (custom_handler) {
    std::string encoded = custom_handler->encode(test_cmd);
    RCLCPP_INFO(logger, "Custom Protocol: %s", encoded.c_str());
  }
  
  // Test JSON Protocol
  auto json_handler = ProtocolFactory::create("json", logger);
  if (json_handler) {
    std::string encoded = json_handler->encode(test_cmd);
    RCLCPP_INFO(logger, "JSON Protocol: %s", encoded.c_str());
  }
  
  // Test Simple Protocol
  auto simple_handler = ProtocolFactory::create("simple", logger);
  if (simple_handler) {
    std::string encoded = simple_handler->encode(test_cmd);
    RCLCPP_INFO(logger, "Simple Protocol: %s", encoded.c_str());
  }
  
  // Test Binary Protocol
  auto binary_handler = ProtocolFactory::create("binary", logger);
  if (binary_handler) {
    std::string encoded = binary_handler->encode(test_cmd);
    RCLCPP_INFO(logger, "Binary Protocol encoded (length: %zu bytes)", encoded.length());
  }
  
  RCLCPP_INFO(logger, "Protocol testing completed");
  
  rclcpp::shutdown();
  return 0;
}